/*
 * PSChangeFormation.java
 *
 * Created on 17 de noviembre de 2007, 12:11 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 *
 *
 *            ----------------
 *       F   /   \RRRRRRRR/   \   F
 *          /     \RRRRRR/     \       
 *         /  G    \RRRR/  G    \
 *        |\        \RR/        /|
 *        |F\        \/        /F|
 *                   C
 *
 *
 * R = RedZone
 * G = GreenZone
 * F = FreeZone
 * C = Centroid of Formation
 */

package Behaviors.Schemas.Perception;
import Behaviors.Formations.FormationGenerator;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.Sensors.Vector2DSensor;
import Utils.Vector2d;
import javaclient2.Position2DInterface;
import Robot.Position2d;
import Robot.Sensors.Sensor;

import Utils.Pose2d;
/**
 *
 * @author alejo
 */
public class PSChangeFormation extends Schema implements Vector2dSchema{
    
    private Position2d centroidFormPos;
    private FormationGenerator formGen;
    private PSDetectObstacles obstaclesPS;
    private PSDetectFormation detectForm;
    private float redZoneAngle = (float) Math.PI / 18.0f; // 10 degrees
    private float greenZoneAngle = (float) Math.PI/2f ; // 70 degrees 
    
    private float distanceZone = 300.0f;
    private float effectiveDistance = 30.0f;
    private float opportunityDistance = 30.0f;
    
    static final int NO_OBST = -1;
    static final int FREE_ZONE= 0;
    static final int OBST_IN_RED_ZONE = 1;
    static final int OBST_IN_GREEN_ZONE_L = 2;
    static final int OBST_IN_GREEN_ZONE_R = 3;
    static final float TAM_ROBOT = 35;
    static final float TAM_OBST = 20;
    
    boolean huboChange = false;
    
    
    
    
    
    
    
    /** Creates a new instance of PSChangeFormation */
    public PSChangeFormation(String name,Position2d centroidFormPos, FormationGenerator formGen, PSDetectObstacles obstaclesPS, PSDetectFormation detectForm) {
        super(name);
        this.centroidFormPos = centroidFormPos;
        this.formGen = formGen;
        this.obstaclesPS = obstaclesPS;
        this.detectForm = detectForm;
        
        
    }
    
    
     public Vector2d Value(){
         
         //flags
         boolean obstInRedZone = false;
         boolean obstInFreeZone = false;
         boolean obstInGreenZoneLeft = false;
         boolean obstInGreenZoneRight = false;
        
         Pose2d centroidPose = centroidFormPos.read(Sensor.READ_MODE_NO_WAIT);
         //Get obstacle positions
         Vector2d[] obstacles = obstaclesPS.Value();
         
         Vector2d[] vectToObst = new Vector2d[obstacles.length];
         int[] obstZone = new int[obstacles.length];
         
         
        //calculate distance from centroid to obstacles
         
         for(int i=0;i<obstacles.length;i++)
         {
             
             if(obstacles[i].getMag() >0){
                 
                //System.out.println("si obst");
                 vectToObst [i] = obstacles[i].sub(centroidFormPos.getPosition());
                 //System.out.println("obstacle mag: " +vectToObst[i].getMag() + " y: " + vectToObst[i].getY());
                 
         
         
                 double obstAngle =vectToObst[i].getAngle() - centroidPose.getPa();
                 
                 //correction of angle
                 obstAngle = Vector2d.angleCorrMPI2PI(obstAngle);
                                
                 /*
                 System.out.print("Theta: ");
                 System.out.print(Math.toDegrees(vectToObst[i].getTheta()));
                 System.out.print(" - Theta ref: ");
                 System.out.println(Math.toDegrees(obstAngle));
                 */

                 //checkout first if obstacle is in zone
                 if(vectToObst[i].getMag()<=distanceZone)
                 {
                      //System.out.println("si en dist");
                     //checkout if obstacles are in redZone

                     if(Math.abs(obstAngle) <= redZoneAngle ){
                         obstZone[i] = OBST_IN_RED_ZONE;
                         obstInRedZone =true;
                     }
                     //checkout if obstacles are in greenzone left (+)
                     else if(obstAngle <= greenZoneAngle && obstAngle>0){
                         obstZone[i] = OBST_IN_GREEN_ZONE_L;
                         obstInGreenZoneLeft =true;
                     }
                     //checkout if obstacles are in greenzone rigth (-)
                     else if(-obstAngle <= greenZoneAngle  && obstAngle<0){
                         obstZone[i] = OBST_IN_GREEN_ZONE_R;

                         obstInGreenZoneRight =true;
                     }
                     else{
                         obstZone[i] = FREE_ZONE;
                         obstInFreeZone = true;
                     }

                 }
                 else{
                     obstZone[i] = FREE_ZONE;
                     obstInFreeZone = true;
                 }
             }
             else{
                 obstZone[i] = NO_OBST;
                 
                 
             }
         }
             
         if(!obstInRedZone){
             
             //deal with obstacles that are on both sides of green area
             if(obstInGreenZoneLeft && obstInGreenZoneRight)
             {
                 //System.out.println("GreenZone");
                 double minXDistLeft = distanceZone;
                 double minXDistRight = distanceZone;
                 double minY = distanceZone;
                 //choose the obstacles that are the nearest on both sides
                 for(int i=0;i<obstacles.length;i++){
                     
                     //save minimal distance to any of the obstacles
                     double distObstY = Math.abs(vectToObst[i].getX());
                     
                         
                     
                     
                     switch(obstZone[i]){
                         case OBST_IN_GREEN_ZONE_L:                     
                             if(Math.abs(vectToObst[i].getY())<minXDistLeft && Math.abs(vectToObst[i].getX())<=opportunityDistance)
                                 minXDistLeft = Math.abs(vectToObst[i].getY());
                             if (distObstY<minY)
                                 minY = distObstY;
                             break;
                         case OBST_IN_GREEN_ZONE_R:

                             if(Math.abs(vectToObst[i].getY())<minXDistRight && Math.abs(vectToObst[i].getX())<=opportunityDistance)
                                 minXDistRight = Math.abs(vectToObst[i].getY());                             
                             if (distObstY<minY)
                                 minY = distObstY;
                    }
                 
                 }
                 //System.out.println("minY: "+minY);
                 //now calculate distance between nearest obstacles on both sides
                 double distance = Math.abs(minXDistLeft)+Math.abs(minXDistRight);
                 
                 //if distance between objects is less than needed, change form
                 
                 if(distance < formGen.getActualFormation().getWidth() && distance > TAM_ROBOT && minY<=effectiveDistance){
                    
                     float MIN_CUADRADO = 55;
                     float MIN_FLECHA = 80;
                     float MIN_FILA = 120;
                      
                     
                     String form = formGen.getActualFormation().getName();
                     distance = distance- TAM_ROBOT - TAM_OBST;
                     if(form.equals("SQUARE"))
                     {
                         if(distance>MIN_CUADRADO)
                         {
                             //calcular ancho 
                             formGen.generatePositions("SQUARE",distance,4);
                             System.out.println("Changing to form SQUARE width width: "+distance);
                         }
                         else
                         {
                             formGen.generatePositions("COLUMN",180,4);
                             System.out.println("Changing to form COLUMN width width: "+distance);

                     }
                     }
                     else if(form.equals("ROW"))
                     {
                         if(distance>MIN_FILA)
                         {
                             //calcular entonces a partir de la dist el cambio de ancho
                             formGen.generatePositions("ROW",distance,4);
                             System.out.println("Changing to form ROW width width: "+distance);
                         }
                         else if(distance>MIN_FLECHA)
                         {
                             formGen.generatePositions("ARROW",distance,4);
                             System.out.println("Changing to form ARROW width width: "+distance);
                         }
                         else if(distance>MIN_CUADRADO)
                         {
                             formGen.generatePositions("SQUARE",distance,4);                         
                             System.out.println("Changing to form SQUARE width width: "+distance);
                         }
                         else
                         {
                             formGen.generatePositions("COLUMN",180,4);
                             System.out.println("Changing to form COLUMN width width: "+distance);

                         }
                     
                     }
                     else if(form.equals("ARROW"))
                     {
                         if(distance>MIN_FLECHA)
                         {
                             formGen.generatePositions("ARROW",distance,4);
                             System.out.println("Changing to form ARROW width width: "+distance);
                         }
                         else if(distance>MIN_CUADRADO)
                         {
                             formGen.generatePositions("SQUARE",distance,4);
                             System.out.println("Changing to form SQUARE width width: "+distance);
                         }
                         else
                         {
                             formGen.generatePositions("COLUMN",180,4);
                             System.out.println("Changing to form COLUMN width width: "+distance);

                         }
                     
                     }
                     else if(form.equals("COLUMN"))
                     {
                         //no hay cambios necesarios aquiformGen.generatePositions("SQUARE",distance,4);
                     
                     }
        
                     huboChange =true;

             
             }
         
         }
         //now deal with obstacles located in red zone
         else if(obstInRedZone){
         
             //System.out.println("RedZone");
         }
             //cases where there is no geometric restrictions in obstacles
         else
         {
                 //System.out.println("NoZone");
                 
                 if(huboChange)
                 {
                     try{
                     Thread.sleep(10000);
                     }
                     catch(Exception e){}
                     huboChange=false;
                 }
                 formGen.returnToCurrentFormation();
                 
                 
         }
         
         }
             
         
         
             
         
         
         
         
         //Define if centroid of formation is in the area where formation change is suspected to ocurre
         
         //Define which formation suits the environment needs
         //First checkout the width of formation, then checkout other formations
         
         
         //Establish new formation
         
         return new Vector2d(0,0,Vector2d.MODE_XY);
     }
    
}
